/**
* Gravity stuff
*/


#include <OGF/quick_start/common/deform.h>
#include <OGF/quick_start/common/select_util.h>
#include <OGF/basic/os/stopwatch.h>
#include <OGF/cells/map/map_cell_heap.h>

#include <set>
#include <stack>
#include <queue>
#include <limits>

#include <OGF/math/linear_algebra/vector.h>

#include <OGF/quick_start/common/property_smoother.h>

namespace OGF 
{
	/**
	* Find alpha, such that (L^T)*alpha = mass
	* L^T is rank deficient, so we have to either use the 
	* pseudo-norm, or a regularized system. Or some other trick.
	* PRE:
	*    - vertex lists are all setup
	* POST:
	*    - alpha vector is ready.
	*/
	void Deform::calc_alpha()
	{
		std::cout<<" Calculating mass-alpha vector. "<<std::endl;

		//Number of rows in the original matrix:
		unsigned int laplace_rows = verts_guard.size() + verts_core.size() + 1;
		unsigned int constraint_rows = verts_guard.size() + verts_border.size() + 1;
		unsigned int n_columns = laplace_rows + constraint_rows;
		
		SystemSolverParameters params ;
       	params.set_arg("method", "SUPERLU") ;
		LinearSolver solver(n_columns);

		solver.set_system_solver(params) ;
		solver.set_least_squares(true) ;
		solver.set_invert_matrix(true) ;
		solver.set_symmetric(false);
		solver.set_quiet(true);
		solver.begin_system() ;

		for(unsigned int i=0;i<verts.size();i++)
		{
			//std::cout<<"Adding transpose row "<<i<<std::endl;
			fill_tranpose_row(solver, verts[i], GRAVITY_COORD);
		}

		double k = 0.000001; //regularization strength

		std::cout<<"  Regularizing "<<std::endl;
		for(unsigned int i=0;i<n_columns; i++)
		{
			solver.begin_row();
			solver.add_coefficient(i, k);
			solver.set_right_hand_side(0);
			solver.end_row();
		}

		solver.end_system();

		std::cout<<"  Solving "<<std::endl;
		solver.solve() ;

		alpha.clear();
		alpha.reserve(n_columns);
		for(unsigned int i=0;i<n_columns;i++)
		{
			alpha.push_back( solver.variable(i).value() );
			//std::cout<<alpha[i]<<std::endl;
		}
	}

	/**
	* Fill in 1 row of the [Laplace|Constraint] matric's transpose. 
	* This bends my mind.
	*/
	void Deform::fill_tranpose_row(LinearSolver &solver, Map::Vertex *vert, int coord)
	{
		// We are dealing with the tranpose of the matrix created in Deform::setup_Laplace()
		// I will refer to row/column as though we were in that matrix still. Thus, this 
		// method is building 1 column (of that matrix).
		// Each column of that matrix has a 1-1 map to a vertex (when the rows are dotted 
		// with the vertex pos vector during multiplcation). That is the vector passed here.
		solver.begin_row();

		// The first part of the matrix is the Laplacian part.
		
		// If 'vert' has a Laplace row, then add a '1' in that row. (that column, in the Transpose)
		if( has_laplace_row(vert) )
			solver.add_coefficient( (*index)[vert], (*laplace_weight)[vert] );

		// Any vertex adjacent to 'vert', which has a laplace row, adds
		// a coefficient in its row (and in our column).
		Map::Vertex *adj;
		FOR_EACH_ADJACENT_VERTEX(vert, adj)
		{
			if( has_laplace_row(adj) )
			{
				double entry = - (*laplace_weight)[adj] / adj->degree();
				solver.add_coefficient( (*index)[adj], entry );
			}
		}

		// The second part of the matrix is the constraints.  Only guard vertices
		// have constraints. If 'vert' has a constraint row, then it puts a 1 in
		// the column we are building.
		if( has_constraint_row(vert) )
		{
			//std::cout<<"  Adding constraint column"<<std::endl;
			// This is tricky. The verts are ordered: anchor, core, guard, border
			// The anchor, core, and guard have Laplace rows.
			// So the first constraint row is (core.size() + guard.size() + 1)
			// But as the first thing to get a constraint is a guard, then
			// it's "constraint index" is (*index) - core.size() - 1.
			// The sum gives the 'constraint row'

			// Long code to make it clear what we are doing:
			//int first_constraint_row = 1 + verts_core.size() + verts_guard.size();
			//int first_constraint_index = 1 + verts_core.size();
			//int my_index = (*index)[vert];
			//int my_index_within_constraints = my_index - first_constraint_index;
			//int constraint_row = first_constraint_row + my_index_within_constraints;

			//short equation:
			int constraint_row = verts_guard.size() + (*index)[vert];
			solver.add_coefficient( constraint_row, this->constraint_weight );
		}

		// Set the RHS:
		solver.set_right_hand_side( (*mass)[vert] );
		solver.end_row();
	}


	/**
	* Assigns mass to each vertex, this is a property of the input mesh.
	*/
	void Deform::calc_masses()
	{

		if( MapVertexAttribute<double>::is_defined(surface, "FEATURE_SIZE") )
		{
			MapVertexAttribute<double> feature_size(surface, "FEATURE_SIZE");		
			FOR_EACH_VERTEX(Map, surface, v)
			{
				double radius = feature_size[v];
				double area = (*laplace_weight)[v];
				(*mass)[v] = radius*area;
			}		
		}
		else
		{
			FOR_EACH_VERTEX(Map, surface, v)
			{
				(*mass)[v] = 1.0;
			}
		}
		PropertySmoother s;
		s.smooth(mass, 3, surface, 0.6);
		normalize_attribute(*mass);
	}

	/**
	* Calculate the weighting term for the Laplacian constraints. 
	* These are NOT the masses, just some constant factors to make 
	* Laplace consistent and stronger or weaker.
	*/
	void Deform::calc_Laplace_weights()
	{	
		FOR_EACH_VERTEX(Map, surface, v)
		{
			Map::Facet *f;
			double weight = 0;
			FOR_EACH_ADJACENT_FACE(v, f)
			{
				ogf_assert(f != nil);
				weight += (*area)[f];
			}
			//weight *= 1000.0; //some scaling constant in units: J/m^3
			(*laplace_weight)[v] = weight;
		}
		PropertySmoother s;
		s.smooth(laplace_weight, 3, surface, 0.6);
		normalize_attribute(*laplace_weight);
	}

	/**
	* Calculate the area of each of the faces.
	*/
	void Deform::calc_areas()
	{
		FOR_EACH_FACET(Map, surface, f)
			(*area)[f] = calc_face_area(f);
	}

	/**
	* Calculate the area of a single face.
	*/
	double Deform::calc_face_area(Map::Facet *f)
	{
		// The area of the parallelogram with sides PQ and PR 
		// is equal to the magnitude of the cross product of 
		// vectors representing two adjacent sides:
		// Area(parallelogram) = |PQ � PR|. The area of the 
		// triangle is half of this.
		
		ogf_assert(f->is_triangle());

		Point3d P = f->halfedge()->vertex()->point();
		Point3d Q = f->halfedge()->prev()->vertex()->point();
		Point3d R = f->halfedge()->next()->vertex()->point();

		Vector3d PQ = P-Q;
		Vector3d PR = P-R;

		double area = 0.5 * (PQ^PR).norm();
		return area;
	}
	
	void Deform::normalize_attribute(MapVertexAttribute<double> &att)
	{
		double sum = 0;
		int N=0;
		FOR_EACH_VERTEX(Map, surface, v)
		{
			sum += att[v];
			N++;
		}
		double average = sum/N;
		FOR_EACH_VERTEX(Map, surface, v)
		{
			att[v] = att[v] / average;
		}
	}

} //namespace